
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mp_roll_controller.h
  * @author     baiyang
  * @date       2023-6-29
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include "mp_autotune.h"

#include <pid/pid.h>
#include <pid/pid_info.h>

#include <vehicle/mb_fixed_wing.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
typedef struct mp_roll_controller* mp_roll_controller_t;

struct mp_roll_controller {
    mb_fixed_wing_const_t aparm;
    struct ATGains gains;
    mp_autotune_t autotune;
    bool failed_autotune_alloc;
    float _last_out;
    PID_ctrl rate_pid;
    float angle_err_deg;
    float _dt;

    struct pid_info _pid_info;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void rctrl_ctor(mp_roll_controller_t roll_ctrl, mb_fixed_wing_const_t parms, float dt);

// set_dt - set time step in seconds
static inline void rctrl_set_dt(mp_roll_controller_t roll_ctrl, float initial_dt) { pid_ctrl_set_dt(&roll_ctrl->rate_pid, initial_dt); }

float rctrl_get_rate_out(mp_roll_controller_t roll_ctrl, float desired_rate, float scaler);
float rctrl_get_servo_out(mp_roll_controller_t roll_ctrl, int32_t angle_err, float scaler, bool disable_integrator, bool ground_mode);

/*
  start an autotune
 */
void rctrl_autotune_start(mp_roll_controller_t roll_ctrl);

/*
  restore autotune gains
 */
void rctrl_autotune_restore(mp_roll_controller_t roll_ctrl);

static inline void rctrl_reset_i(mp_roll_controller_t roll_ctrl) { pid_ctrl_reset_i(&roll_ctrl->rate_pid); }

/*
  reduce the integrator, used when we have a low scale factor in a quadplane hover
*/
static inline void rctrl_decay_i(mp_roll_controller_t roll_ctrl) {
    // this reduces integrator by 95% over 2s
    roll_ctrl->_pid_info.I *= 0.995f;
    pid_ctrl_set_integrator1(&roll_ctrl->rate_pid, roll_ctrl->rate_pid.integrator * 0.995f);
}

static inline pid_info_const_t rctrl_get_pid_info(mp_roll_controller_t roll_ctrl) { return &roll_ctrl->_pid_info; }
static inline float rctrl_get_kp(mp_roll_controller_t roll_ctrl) { return roll_ctrl->rate_pid._kp; }
static inline float rctrl_get_ki(mp_roll_controller_t roll_ctrl) { return roll_ctrl->rate_pid._ki; }
static inline float rctrl_get_kd(mp_roll_controller_t roll_ctrl) { return roll_ctrl->rate_pid._kd; }
static inline float rctrl_get_kff(mp_roll_controller_t roll_ctrl) { return roll_ctrl->rate_pid._kff; }
static inline float rctrl_get_tau(mp_roll_controller_t roll_ctrl) { return roll_ctrl->gains.tau; }
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



